/** Joystick.cpp
Title   : Joystick.cpp
Author  : Manikantan Nambi
URL :
Description : Function definitions for Joystick.h.
Created : 1/1/11
Modified   : 2/14/11
*/

#include <QWidget>
#include <QtDebug>

#include "Joystick.h"
using namespace std;

/**
   Parameters:     parent widget
   Returns:    None
*/

Joystick::Joystick(QWidget *parent) : QWidget(parent) {

    qDebug()<<"Joystick:Joystick(): Initializing device";

    if(spnav_open()==-1)
    {
        qDebug()<<"Joystick:Joystick():failed to connect to the space navigator daemon\n";
    } else {
        qDebug()<<"Joystick:Joystick(): Initializing done ...";
    }

    x=0;
    y=0;
    z=0;
    rx=0;
    ry=0;
    rz=0;


}

/*
*   Summary:    Joystick class destructor
*
*   Parameters:     None
*   Returns:    None
*/
Joystick::~Joystick()
{
    close();
}

/**
*   Summary:    Joystick class open function
*
*   Parameters:     None
*   Returns:    True or False depending on if driver is opened
*/
int Joystick::open()
{
//    spnav_close();
    if(spnav_open()==-1)
    {
        qDebug()<<"Joystick:Joystick():failed to connect to the space navigator daemon\n";
        return -1;
    }
    else
        return 1;
}

/**
*   Summary:    Joystick class close function
*
*   Parameters:     None
*   Returns:    None
*/
void Joystick::close()
{
//    spnav_close();
}

/**
*   Summary:    polls device for the state of the device
*
*   Parameters:     None
*   Returns:    None
*/
void Joystick::pollDevice()
{

    spnav_poll_event(&dEvent);


//    qDebug()<< "Even type"<<dEvent.type;
    if(dEvent.type == SPNAV_EVENT_MOTION)
    {
//        qDebug()<<"got motion event: t"<< dEvent.motion.x<< dEvent.motion.y<< dEvent.motion.z;

        x = dEvent.motion.x;
        y = dEvent.motion.z;
        z = dEvent.motion.y;

        rx = dEvent.motion.rx;
        ry = dEvent.motion.ry;
        rz = dEvent.motion.rz;

        dEvent.type = 0;

        //qDebug()<< "Norm"<<sqrt(x*x+y*y)<< "direction"<<atan2(-y,-x);
    }
    else if(dEvent.type==dEvent.button.press)
    {
        button = dEvent.button.bnum;
    }
    else
    {
        button = -1;
        x = 0;
        y = 0;
        z = 0;
        //qDebug()<<"got motion event: t"<< x<< y<< z;
        //qDebug()<<"got button event"<<button;

    }


    // flushing event type so that x,y,z, and button are reset when there is no input
    dEvent.type = 0;
    spnav_remove_events(SPNAV_EVENT_MOTION);

}

double Joystick::velMag()
{
    return(sqrt(x*x + y*y));
}

double Joystick::velDir()
{
    return(atan2(y,x));
}
